/**
 * This is my own wrapped GLM library, I just use it temporarily
 * Actually, we should use the one here http://sourceforge.net/projects/ogl-math/files/
 * */

#include <math.h>
#include "matrix.h"
#include "nmath/funct.h"

void setIdentity(float *m){
	m[0] = 1.0f;
	m[1] = 0;
	m[2] = 0;
	m[3] = 0;

	m[4] = 0;
	m[5] = 1.0f;
	m[6] = 0;
	m[7] = 0;

	m[8] = 0;
	m[9] = 0;
	m[10] = 1.0f;
	m[11] = 0;

	m[12] = 0;
	m[13] = 0;
	m[14] = 0;
	m[15] = 1.0f;
}

void rotateX(float theta, float *m){
	float cosx = cos(theta);
	float sinx = sin(theta);
	/*
			{1,	0, 		0, 		0},
			{0,	cosx, 	sinx, 	0},
			{0,	-sinx, 	cosx, 	0},
			{0,	0, 		0, 		1}};*/
	m[0] = 1;
	m[1] = 0;
	m[2] = 0;
	m[3] = 0;

	m[4] = 0;
	m[5] = cosx;
	m[6] = sinx;
	m[7] = 0;

	m[8] = 0;
	m[9] = -sinx;
	m[10] = cosx;
	m[11] = 0;

	m[12] = 0;
	m[13] = 0;
	m[14] = 0;
	m[15] = 1;
}

void rotateY(float theta, float *m){
	float cosx = cos(theta);
	float sinx = sin(theta);
	/*
		{cosx, 	0, 	-sinx, 	0},
		{0, 	1, 		0, 	0},
		{sinx, 	0, 	 cosx, 	0},
		{0, 	0, 		0, 	1}};*/
	m[0] = cosx;
	m[1] = 0;
	m[2] = -sinx;
	m[3] = 0;

	m[4] = 0;
	m[5] = 1;
	m[6] = 0;
	m[7] = 0;

	m[8] = sinx;
	m[9] = 0;
	m[10] = cosx;
	m[11] = 0;

	m[12] = 0;
	m[13] = 0;
	m[14] = 0;
	m[15] = 1;
}

void rotateZ(float theta, float *m){
	float cosx = cos(theta);
	float sinx = sin(theta);
	/*
		{cosx, 	sinx, 	0, 	0},
		{-sinx, cosx, 	0, 	0},
		{0, 	0, 		1, 	0},
		{0, 	0, 		0, 	1}};
	*/
	m[0] = cosx;
	m[1] = sinx;
	m[2] = 0;
	m[3] = 0;

	m[4] = -sinx;
	m[5] = cosx;
	m[6] = 0;
	m[7] = 0;

	m[8] = 0;
	m[9] = 0;
	m[10] = 1;
	m[11] = 0;

	m[12] = 0;
	m[13] = 0;
	m[14] = 0;
	m[15] = 1;
}

/*
 * angle rotation in degree
 *
 * */
int rotateM(float *rm, float *m, float a, float x, float y, float z){
	float s, c, d;
	float R[16];
	float recipLen,nc, xy, yz, zx, xs, ys, zs;

	R[3] = 0;
	R[7] = 0;
	R[11] = 0;
	R[12] = 0;
	R[13] = 0;
	R[14] = 0;
	R[15] = 1;
	a *= (PI / 180.0f);
	s = sin(a);
	c = cos(a);
	d = sqrt(x*x + y*y + z*z);

	if(x == 1.0f && y == 0.0f && z == 0.0f){
		 R[5] = c;   R[10]= c;
         R[6] = s;   R[9] = -s;
         R[1] = 0;   R[2] = 0;
         R[4] = 0;   R[8] = 0;
         R[0] = 1;
	}else if(x == 0.0f && y == 1.0f && z == 0.0f){
		R[0] = c;   R[10]= c;
        R[8] = s;   R[2] = -s;
        R[1] = 0;   R[4] = 0;
        R[6] = 0;   R[9] = 0;
        R[5] = 1;
	} else if (0.0f == x && 0.0f == y && 1.0f == z) {
		R[0] = c;   R[5] = c;
        R[1] = s;   R[4] = -s;
        R[2] = 0;   R[6] = 0;
        R[8] = 0;   R[9] = 0;
        R[10]= 1;
    } else {
        if (1.0f != d) {
        	recipLen = 1.0f / d;
            x *= recipLen;
            y *= recipLen;
            z *= recipLen;
        }
        nc = 1.0f - c;
        xy = x * y;
        yz = y * z;
        zx = z * x;
        xs = x * s;
        ys = y * s;
        zs = z * s;

        R[0] = x*x*nc +  c;
        R[4] =  xy*nc - zs;
        R[8] =  zx*nc + ys;
        R[1] =  xy*nc + zs;
        R[5] = y*y*nc +  c;
        R[9] =  yz*nc - xs;
        R[2] =  zx*nc - ys;
        R[6] =  yz*nc + xs;
        R[10] = z*z*nc +  c;
    }

	multiplyMM(m, R, rm);
}

/*
 *  Calculate R = A * B
 * */
int multiplyMM(float *R, float *A, float *B){
	double s;
	int i, j, k;

	for(i=0; i < 4; i++){
		for(j=0; j < 4; j++){
			s = 0;
			for(k=0; k < 4; k++)
				s += A[i*4+k] * B[k*4+j];
			R[i*4+j] = s;
		}
	}
	return 1;
}

/*
 * Return 1 if input ok, otherwise return 0
 *
 *
 * */
int frustumM(float *m, float left, float top, float right, float bottom, float near, float far){
	float r_width, r_height, r_depth;
	if(near == far)
		return 0;

	r_width  = 1.0f / (right - left);
	r_height = 1.0f / (top - bottom);
    r_depth  = 1.0f / (near - far);

    /* fixed the bug of android as mention here http://code.google.com/p/android/issues/detail?id=35646 */
    m[0] = (near * r_width);
    m[5] = 2.0f * (near * r_height);
    m[8] = 2.0f * ((right + left) * r_width);
    m[9] = (top + bottom) * r_height;
    m[10] = (far + near) * r_depth;
    m[14] = 2.0f * (far * near * r_depth);
    m[11] = -1.0f;
    m[1] = 0.0f;
    m[2] = 0.0f;
    m[3] = 0.0f;
    m[4] = 0.0f;
    m[6] = 0.0f;
    m[7] = 0.0f;
    m[12] = 0.0f;
    m[13] = 0.0f;
    m[15] = 0.0f;

    return 1;
}
